/*
 * This file is part of the Try! Engine project.
 *
 * Copyright 2010-2011 Emanuele Bertoldi. All rights reserved.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * You should have received a copy of the modified BSD License along with this
 * program. If not, see <http://www.opensource.org/licenses/bsd-license.php>
 */

#include "motionstate.h"

using namespace Try;

MotionState::MotionState(Transformable* node, const btTransform& centerOfMass)
    :   m_node(node),
        m_centerOfMass(centerOfMass)
{
}

MotionState::~MotionState()
{
    m_node = 0;
}

void MotionState::getWorldTransform(btTransform& centerOfMassWorldTrans) const
{
    if (m_node)
    {
        Transform t = m_node->transform();
        btTransform bt = MotionState::convert(t);
        centerOfMassWorldTrans = m_centerOfMass.inverse() * bt;
    }
}

void MotionState::setWorldTransform(const btTransform& centerOfMassWorldTrans)
{
    if (m_node)
    {
        btTransform bt = centerOfMassWorldTrans * m_centerOfMass;
        Transform t = MotionState::convert(bt);
        m_node->setTransform(t);
    }
}

Transform MotionState::convert(const btTransform& t)
{
    Transform output;

    btVector3 pos = t.getOrigin();
    btQuaternion rot = t.getRotation();

    output.position = Vector3(pos.x(), pos.y(), pos.z());
    output.rotation = Quaternion(rot.x(), rot.y(), rot.z(), rot.w());

    return output;
}

btTransform MotionState::convert(const Transform& t)
{
    btTransform output;

    Vector3 pos = t.position;
    Quaternion rot = t.rotation;

    output.setOrigin(btVector3(pos.x, pos.y, pos.z));
    output.setRotation(btQuaternion(rot.x, rot.y, rot.z, rot.w));

    return output;
}
